#include <iostream>
#include <opencv2/opencv.hpp>
#include <thread>
#ifndef USE_NVIDIA
#include "orin_detect.h"
#else
#include "ov_detect.h"
#endif
#include "detector.h"
#include "GrabImg.h"
#include "common_structs.h"
#include "my_serial.h"
#include "main.h"

using namespace std;
using namespace cv;

enum E_CAMERA_RESULT 
{
    NO_IMAGE = 0,
    SUCCESS,
    CAMERA_ERROR
};

bool if_show = 1;

Appconfig config;

#ifndef USE_NVIDIA
Nv_Detector nv_detector;
cv::Mat left_frame;
cv::Mat right_frame;
toe::serial_port temp_left("/dev/ttyACM0");
toe::serial_port temp_right("/dev/ttyACM1");
#else
toe::serial_port templ;
OvO_Detector ov_detector;
#endif

#ifndef USE_NVIDIA
void grab_left_(void)
{
    s_camera_params cam;
    cam.device_id = 0;
    HikGrab hik(cam);
    hik.Hik_init();
    hik.Hik_end();
    sleep(1);
    hik.Hik_init();
    while (1)
    {
        if (E_CAMERA_RESULT::SUCCESS == nv_detector.left) 
        {
            usleep(10);
            continue;
        }
        if (hik.get_one_frame(left_frame, 0))
        {
            nv_detector.left = E_CAMERA_RESULT::SUCCESS;
        }
        else
        {
            nv_detector.left = E_CAMERA_RESULT::CAMERA_ERROR;
            hik.Hik_init();
            hik.Hik_end();
            sleep(1);
            hik.Hik_init();
            
        }
        //cout << "left: " << nv_detector.left << endl;
    }
    hik.Hik_end();
}
void grab_right_(void)
{
    s_camera_params cam;
    cam.device_id = 1;
    HikGrab hik(cam);
    hik.Hik_init();
    hik.Hik_end();
    sleep(1);
    hik.Hik_init();
    while (1)
    {
        if (E_CAMERA_RESULT::SUCCESS == nv_detector.right) 
        {
            usleep(10);
            continue;
        }
        if (hik.get_one_frame(right_frame, 1))
        {
            nv_detector.right = E_CAMERA_RESULT::SUCCESS;
        }
        else
        {
            nv_detector.right = E_CAMERA_RESULT::CAMERA_ERROR;

            hik.Hik_init();
            hik.Hik_end();
            sleep(1);
            hik.Hik_init();
        }
    }
    hik.Hik_end();
}
void detect_(void)
{
    Appconfig* ptr = &config;
    nv_detector.Init(ptr);
    nv_detector.Nv_Init();
    auto start = std::chrono::high_resolution_clock::now();
    auto end = std::chrono::high_resolution_clock::now();
    float elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
    int fps;
    while(!temp_left.read_port(nv_detector.param_.camp, nv_detector.now_pri_));
    while(!temp_right.read_port(nv_detector.param_.camp, nv_detector.now_pri_));
    /*
    Mat img = imread("/home/nvidia/toe_aimbot_2023/detector/jetson_orin/538.png");
    Mat img1 = imread("/home/nvidia/toe_aimbot_2023/detector/jetson_orin/533.jpg");
    resize(img, img, Size(640,640));
    resize(img1, img1, Size(640,640));
    */
    while (0)
    {
        end =  std::chrono::high_resolution_clock::now();
        elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        fps++;
        //cout << elapsed << endl;
        if (elapsed/1000 > 1000)
        {
            cout << fps << endl;
            start = end;
            fps = 0;
        }
        //nv_detector.input_img_ = img;
        //nv_detector.input_img2_ = img;
            
        nv_detector.detect();
        imshow("0", nv_detector.show_left);
        imshow("1", nv_detector.show_right);
        waitKey(0);

    }

    int write_num=0;
    while (1)
    {
        end =  std::chrono::high_resolution_clock::now();
        elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        
        
        if (elapsed/1000 > 1000)
        {
            cout << fps << endl;
            start = end;
            fps = 0;
            temp_left.read_port(nv_detector.param_.camp, nv_detector.now_pri_);
            temp_right.read_port(nv_detector.param_.camp, nv_detector.now_pri_);
        }
        // cout << nv_detector.left << " " << nv_detector.right << endl;
        /*
        if (nv_detector.left && nv_detector.right)
        {
            cout << "1" << endl;
            nv_detector.left = 0;
            nv_detector.right = 0;
            continue;
        }
        */
        /* 相机错误或者成功获取到图像 */
        if (nv_detector.left && nv_detector.right)
        {   
            /* 两个相机都掉线则线程延时，降低CPU使用率 */
            if (E_CAMERA_RESULT::CAMERA_ERROR == nv_detector.left 
                && E_CAMERA_RESULT::CAMERA_ERROR == nv_detector.right)
            {
                if (fps == 50)
                    cout << "CAMERA ALL ERROR!!!!!!!!" << endl;
                sleep(1);
                continue;
            }
            else if (E_CAMERA_RESULT::CAMERA_ERROR == nv_detector.left)
            {
                if (fps == 50)
                    cout << "CAMERA LEFT ERROR!!!!!!!!" << endl;
                nv_detector.right = E_CAMERA_RESULT::NO_IMAGE;
                nv_detector.input_img2_ = right_frame.clone();
            }
            else if (E_CAMERA_RESULT::CAMERA_ERROR == nv_detector.right)
            {
                if (fps == 50)
                    cout << "CAMERA RIGHT ERROR!!!!!!!!" << endl;
                nv_detector.left = E_CAMERA_RESULT::NO_IMAGE;
                nv_detector.input_img_ = left_frame.clone();
            }
            else
            {
                nv_detector.left = E_CAMERA_RESULT::NO_IMAGE;
                nv_detector.right = E_CAMERA_RESULT::NO_IMAGE;
                nv_detector.input_img_ = left_frame.clone();
                nv_detector.input_img2_ = right_frame.clone();
            }
            fps++;
            //cout << left << " " << right << endl;
            nv_detector.detect();

            if (E_CAMERA_RESULT::CAMERA_ERROR != nv_detector.right)
            {
                temp_right.write_port(nv_detector.tracker[9].x_c, nv_detector.tracker[9].y_c, nv_detector.tracker[9].z, nv_detector.tracker[9].type);
            }
            if (E_CAMERA_RESULT::CAMERA_ERROR != nv_detector.left)
            {
                temp_left.write_port(nv_detector.tracker_right[9].x_c, nv_detector.tracker_right[9].y_c, nv_detector.tracker_right[9].z, nv_detector.tracker_right[9].type);
            }
            //cout << nv_detector.tracker_right[9].x_c << endl;

            if (if_show)
            {
                if (fps == 50) 
                {
                    //imwrite(to_string(write_num)+"l.png", nv_detector.show_left);
                    //imwrite(to_string(write_num)+"r.png", nv_detector.show_right);
                    //write_num ++;
                }
                imshow("0", nv_detector.show_left);
                imshow("1", nv_detector.show_right);
                waitKey(1);
            }


        }
#if 0
        if (0 && !nv_detector.input_imgs_.empty() && !nv_detector.input_imgs2_.empty())
        {
            nv_detector.input_img_ = nv_detector.input_imgs_.back();
            nv_detector.input_img2_ = nv_detector.input_imgs2_.back();
            //nv_detector.detect();
            //nv_detector.tracker[9].z = 300;
            //nv_detector.tracker_right[9].z = 300;
            //cout << nv_detector.param_.z_scale << " " << nv_detector.param_.z_scale_right << endl;
            //cout << nv_detector.tracker[9].z << " " << nv_detector.tracker_right[9].z << endl;
            temp_left.write_port(nv_detector.tracker[9].x_c, nv_detector.tracker[9].y_c, nv_detector.tracker[9].z, nv_detector.tracker[9].color);
            temp_right.write_port(nv_detector.tracker_right[9].x_c, nv_detector.tracker_right[9].y_c, nv_detector.tracker_right[9].z, nv_detector.tracker_right[9].color);
            nv_detector.show_left = nv_detector.input_img_.clone();
            nv_detector.show_right = nv_detector.input_img2_.clone();
            
            //nv_detector.show_results(nv_detector.show_left, nv_detector.outputs_);
            //nv_detector.show_results(nv_detector.show_right, nv_detector.outputs2_);
            if (if_show)
            {
                imshow("0", nv_detector.show_left);
                imshow("1", nv_detector.show_right);
                waitKey(1);
            }
        }
#endif
        //nv_detector.img_mutex_right_.unlock();
        //nv_detector.img_mutex_left_.unlock();
    }
    
}
int main()
{
    string json_path = "../detector/settings.json";
    load_config(config, json_path);
    //int mode_left = 0;
    //int color_left = 0;//, mode_right, color_right;
    //while(!temp_left.read_port(config.base_config.camp, config.base_config.));
    //while(!temp_right.read_port(color_right, mode_right));
    thread grab_left  = thread(&grab_left_);
    grab_left.detach();
    sleep(1);
    thread grab_right  = thread(&grab_right_);
    grab_right.detach();
    thread detect_thread = thread(&detect_);
    detect_thread.detach();
    while(1)
    {
        /* while 1 循环需要加延时，不然主线程CPU占用率会一直很高 */
        sleep(1000);
    }
    return 0;
}
#else
void detect_(void)
{
    
    Appconfig* ptr = &config;
    ov_detector.Init(ptr);
    ov_detector.OvO_Init();
    
    // video
    /*
    VideoCapture cap("/home/cheakf/toe_aimbot_2023/video/6.avi");
    
    //VideoWriter writer(
    //    "./output.avi", VideoWriter::fourcc('M','J','P','G'), cap.get(CAP_PROP_FPS), Size(640,640)
    //);
    if (!cap.isOpened())
    {
        cout << "Error opening video file" << endl;
        exit(1);
    }
    double fps = cap.get(CAP_PROP_FPS);
    int total_frames = cap.get(CAP_PROP_FRAME_COUNT);
    namedWindow("Video", WINDOW_NORMAL);
    Mat frame;
    int FPS = 0;
    auto infer_start = std::chrono::steady_clock::now();
    while (1)
    {
        //auto start = std::chrono::steady_clock::now();
        bool ret = cap.read(frame);
        if (!ret) break;
        ov_detector.push_img(frame);
        //auto end = std::chrono::steady_clock::now();
        //auto dur = std::chrono::duration<double, milli>(end-start).count();
        // cout << "dur" << dur << endl;
        ov_detector.detect();
        cv::resize(frame, frame, cv::Size(640,640));
        if (ov_detector.output_nms_.size() > 0)
        {
            s_armor armor = ov_detector.get_results(ov_detector.output_nms_);
            circle(frame, Point(armor.x_c, armor.y_c), 3, Scalar(0,255,0), -1);
            //cout << armor.x_c << armor.y_c << endl;
        }
        ov_detector.show_results(frame);
        // writer << frame;
        imshow("Video", frame);
        auto infer_end = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration<double, milli>(infer_end-infer_start).count();
        if (duration > 1000) 
        {
            cout <<"FPS: " << FPS << endl;
            infer_start = infer_end;
            FPS = 0;
        }
        FPS++;
        waitKey(1000);
    }
    cout << "done" << endl;
    cap.release();
    destroyAllWindows();
    */
    int fps = 0;
    auto infer_start = std::chrono::steady_clock::now();
    while (1)
    {
        auto infer_end = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration<double, milli>(infer_end-infer_start).count();
        if (duration > 1000) 
        {
            cout <<"FPS: " << fps << endl;
            infer_start = infer_end;
            fps = 0;
        }
        fps ++;
        ov_detector.detect();
        s_armor armor = ov_detector.get_results(ov_detector.output_nms_);
        cout << armor.z << endl;
        templ.write_port(armor.x_c, armor.y_c, armor.z, armor.type);
        // cout << templ.write_port(armor.x_c, armor.y_c, armor.z, armor.type) << endl;;

    }
}

void grab_(void)
{
    s_camera_params cam;
    cam.device_id = 0;
    HikGrab hik(cam);
    hik.Hik_init();
    hik.Hik_end();
    sleep(2);
    hik.Hik_init();
    auto infer_start = std::chrono::steady_clock::now();
    int fps=0;
    while (1)
    {
        img_mutex.lock();
        Mat s = img_rgb_;
        img_mutex.unlock();
        
        
        if(s.size[0] > 0)
        {
            /*
            auto infer_end = std::chrono::steady_clock::now();
            auto duration = std::chrono::duration<double, milli>(infer_end-infer_start).count();
            if (duration > 1000) 
            {
                cout <<"FPS: " << fps << endl;
                infer_start = infer_end;
                fps = 0;
            }
            fps ++;
            */
            ov_detector.push_img(s);
            //ov_detector.detect();
            ov_detector.show_results(s);
            imshow("1", s);
            waitKey(1);
        }
        // else cout << "no img" << endl;

    }
    hik.Hik_end();
    destroyAllWindows();
}

int main()
{
    string json_path = "../detector/settings.json";
    load_config(config, json_path);
    int mode = 0;
    int color = 0;
    while(!templ.read_port(color, mode));
    config.base_config.camp = color;
    thread grab_thread = thread(&grab_);
    grab_thread.detach();
    thread detect_thread = thread(&detect_);
    detect_thread.detach();
    while(1){;}
    return 0;
}
#endif